////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//	MathQuaternion.cpp
//  MathLib
//
//  Created by Gregory Maks on 8/14/09.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "Math.h"
#include <stdio.h>
#include <math.h>
//#include <cmath>



// ================= Quaternion ===================
void Quaternion::ctor ()
{
	x = 1.0; y = z = 0.0;
	w = 0.0;
}

Quaternion::Quaternion()
{
	InitWithIdentity();
}

Quaternion::Quaternion(Vector4 &axis, floatType angle)
{
	InitWithRotationAroundAxis (axis.x, axis.y, axis.z, angle);
}

Quaternion::Quaternion(floatType axisX, floatType axisY, floatType axisZ, floatType angle)
{
	InitWithRotationAroundAxis (axisX, axisY, axisZ, angle);
}

// creation methods
void Quaternion::InitWithIdentity ()
{
	ctor();
}

void Quaternion::InitWithRotationAroundAxis (Vector4 &axis, floatType angle)
{
	InitWithRotationAroundAxis (axis.x, axis.y, axis.z, angle);
}

void Quaternion::InitWithRotationAroundAxis (floatType axisX, floatType axisY, floatType axisZ, floatType angle)
{
	if (axisX == 0.0 && axisY == 0.0 && axisZ == 0.0)
	{
		ctor();
	}
	else
	{
		floatType sinT = sin(angle/2);
		floatType cosT = cos(angle/2);
		
		x = axisX * sinT;
		y = axisY * sinT;
		z = axisZ * sinT;
		w = cosT;
	}
}


// methods
void Quaternion::Multiply (Quaternion &quat)
{	
	Quaternion resQuat;
	
	resQuat.w = w*quat.w - x*quat.x - y*quat.y - z*quat.z;
	resQuat.x = w*quat.x + x*quat.w + y*quat.z - z*quat.y;
	resQuat.y = w*quat.y + y*quat.w + z*quat.x - x*quat.z;
	resQuat.z = w*quat.z + z*quat.w + x*quat.y - y*quat.x;
	
	*this = resQuat;
}

void Quaternion::Normalize ()
{
	floatType n = sqrt(x*x + y*y + z*z + w*w);
	x /= n;
	y /= n;
	z /= n;
	w /= n;
}

floatType Quaternion::Magnitude ()
{
	return sqrt(x*x + y*y + z*z + w*w);
}


// debug methods
void Quaternion::Printf ()
{
	printf("Quaternion - x:%.2f  y:%.2f  z:%.2f  w:%.2f\n", x, y, z, w);
}


// operators
Quaternion Quaternion::operator* (Quaternion &quat)
{
	Quaternion resQuat;
	
	resQuat.w = w*quat.w - x*quat.x - y*quat.y - z*quat.z;
	resQuat.x = w*quat.x + x*quat.w + y*quat.z - z*quat.y;
	resQuat.y = w*quat.y + y*quat.w + z*quat.x - x*quat.z;
	resQuat.z = w*quat.z + z*quat.w + x*quat.y - y*quat.x;
	
	return resQuat;
}

Quaternion& Quaternion::operator*= (Quaternion &quat)
{
	Multiply(quat);
	return *this;
}

bool Quaternion::operator== (Quaternion &quat)
{
	return (x==quat.x && y==quat.y && z==quat.z && w==quat.w);
}

bool Quaternion::operator!= (Quaternion &quat)
{
	return !(*this == quat);
}








